【ROS】ROS2编程示例：动作 |
您所在的位置：网站首页 › ros 书籍 › 【ROS】ROS2编程示例：动作 |
|
1、准备
1）安装ROS2 【ROS】Ubuntu22.04安装ROS2（Humble Hawksbill） 2）ROS2命令 【ROS】ROS2命令行工具详解 3）配置工作空间 【ROS】ROS2中的概念和名词解释中第一节：工作空间 workspace 4）ROS2源码及官方示例： https://github.com/ros2 https://github.com/ros2/examples 2、编辑 2.1 创建功能包 cd ~/ros/src mkdir laoerExample cd laoerExample ros2 pkg create --build-type ament_cmake cAction --dependencies rclcpp rclcpp_action example_interfaces注意：创建功能包时一定要先创建好目录，进入相应的目录后创建，不要使用下面的方法，否则会在编译时报错 cd ~/ros/src ros2 pkg create --build-type ament_cmake laoerExample/cAction 2.2 编辑源码 2.2.1 动作消息定义动作消息引用ros源码自带的example_interfaces中的Fibonacci.action，内容如下： cat /opt/ros/humble/share/example_interfaces/action$ cat Fibonacci.action # Goal int32 order --- # Result int32[] sequence --- # Feedback int32[] sequence 2.2.2 动作服务器 ~/ros/src/laoerExample/cAction/src$ vi member_functions_srv.cpp #include #include #include #include "example_interfaces/action/fibonacci.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" # a）自定义类MinimalActionServer，继承自节点类rclcpp::Node class MinimalActionServer : public rclcpp::Node { public: # b）using重命名Fibonacci动作、GoalHandleFibonacci目标交互类 using Fibonacci = example_interfaces::action::Fibonacci; using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; # c）构造函数：构造时给节点命名为"minimal_action_server" explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("minimal_action_server", options) { # d）std::placeholders占为符，用于std::bind函数最后的参数 std::placeholders::_1 using namespace std::placeholders; # e）创建服务，参数有好多个，以后用多了会慢慢习惯， # 前四个基本为固定的（设置时钟、日志等）， # 第五个为动作名称 # 后三个为回调函数：接受、取消、执行 this->action_server_ = rclcpp_action::create_server( this->get_node_base_interface(), this->get_node_clock_interface(), this->get_node_logging_interface(), this->get_node_waitables_interface(), "fibonacci", # f）std::bind就是封装下函数，成为函数对象，这样就可以作为参数传递了 std::bind(&MinimalActionServer::handle_goal, this, _1, _2), std::bind(&MinimalActionServer::handle_cancel, this, _1), std::bind(&MinimalActionServer::handle_accepted, this, _1)); } private: rclcpp_action::Server::SharedPtr action_server_; # g）每当有客户端接入时，先调同这个，表示收到命令，返回是否执行 rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); (void)uuid; # h） 限制斐波那契数列计算次数9000 if (goal->order > 9000) { return rclcpp_action::GoalResponse::REJECT; } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } # i）如果客户端主动取消，调用这个函数 rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr goal_handle) { RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } # j）执行函数，需要将这个函数放入单独的线程中 void execute(const std::shared_ptr goal_handle) { RCLCPP_INFO(this->get_logger(), "Executing goal"); rclcpp::Rate loop_rate(1); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); auto & sequence = feedback->sequence; sequence.push_back(0); sequence.push_back(1); auto result = std::make_shared(); for (int i = 1; (i order) && rclcpp::ok(); ++i) { if (goal_handle->is_canceling()) { result->sequence = sequence; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Goal Canceled"); return; } sequence.push_back(sequence[i] + sequence[i - 1]); // Publish feedback goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), "Publish Feedback"); loop_rate.sleep(); } if (rclcpp::ok()) { result->sequence = sequence; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Goal Succeeded"); } } void handle_accepted(const std::shared_ptr goal_handle) { using namespace std::placeholders; # k）这需要快速返回以避免阻塞执行器，因此启动一个新线程 std::thread{std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach(); } }; // class MinimalActionServer int main(int argc, char ** argv) { # A）初始化 rclcpp::init(argc, argv); # B）创建服务并启动 auto action_server = std::make_shared(); # C）spin()创建一个单线程执行器，处理回调任务，服务于指定的节点。它生效后，周期运行。无限阻塞状态。 rclcpp::spin(action_server); # D）关闭 rclcpp::shutdown(); return 0; } 2.2.2 动作客户端 ~/ros/src/laoerExample/cAction/src$ cat member_functions_cli.cpp #include #include #include #include #include #include #include "example_interfaces/action/fibonacci.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" class MinimalActionClient : public rclcpp::Node { public: # a）重命名 using Fibonacci = example_interfaces::action::Fibonacci; using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle; # b）构造函数：设置节点名为"minimal_action_client" explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) : Node("minimal_action_client", node_options), goal_done_(false) { # c）创建动作客户端 this->client_ptr_ = rclcpp_action::create_client( this->get_node_base_interface(), this->get_node_graph_interface(), this->get_node_logging_interface(), this->get_node_waitables_interface(), "fibonacci"); # d）create_wall_timer：定时器，定时500毫米，执行send_goal函数 this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&MinimalActionClient::send_goal, this)); } bool is_goal_done() const { return this->goal_done_; } void send_goal() { using namespace std::placeholders; # e）先定制定时，再执行下面的动作 this->timer_->cancel(); this->goal_done_ = false; if (!this->client_ptr_) { RCLCPP_ERROR(this->get_logger(), "Action client not initialized"); } # f）等待服务端启动 if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); this->goal_done_ = true; return; } auto goal_msg = Fibonacci::Goal(); goal_msg.order = 10; RCLCPP_INFO(this->get_logger(), "Sending goal"); # g）设置回调函数：goal_response_callback、feedback_callback、result_callback # action 由三个部分组成：目标(goal)、反馈（feedback）和结果（result）。 auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.goal_response_callback = std::bind(&MinimalActionClient::goal_response_callback, this, _1); send_goal_options.feedback_callback = std::bind(&MinimalActionClient::feedback_callback, this, _1, _2); send_goal_options.result_callback = std::bind(&MinimalActionClient::result_callback, this, _1); auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options); } private: rclcpp_action::Client::SharedPtr client_ptr_; rclcpp::TimerBase::SharedPtr timer_; bool goal_done_; void goal_response_callback(GoalHandleFibonacci::SharedPtr goal_handle) { if (!goal_handle) { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); } else { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); } } void feedback_callback( GoalHandleFibonacci::SharedPtr, const std::shared_ptr feedback) { RCLCPP_INFO( this->get_logger(), "Next number in sequence received: %" PRId32, feedback->sequence.back()); } void result_callback(const GoalHandleFibonacci::WrappedResult & result) { this->goal_done_ = true; switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); return; case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); return; default: RCLCPP_ERROR(this->get_logger(), "Unknown result code"); return; } RCLCPP_INFO(this->get_logger(), "Result received"); for (auto number : result.result->sequence) { RCLCPP_INFO(this->get_logger(), "%" PRId32, number); } } }; // class MinimalActionClient int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto action_client = std::make_shared(); while (!action_client->is_goal_done()) { # h）rclcpp::spin_some在线程中只执行一次？ rclcpp::spin_some(action_client); } rclcpp::shutdown(); return 0; } 2.3 添加依赖配置如果在创建包时已经指定： ros2 pkg create --build-type ament_cmake cAction --dependencies rclcpp rclcpp_action example_interfaces则下面的内容会自动添加在package.xml中 rclcpp rclcpp_action example_interfaces完整的内容如下 ~/ros/src/laoerExample/cAction$ cat package.xml cAction 0.0.0 TODO: Package description lesen TODO: License declaration ament_cmake rclcpp example_interfaces rclcpp_action ament_lint_auto ament_lint_common ament_cmake 2.4 修改编译配置1）添加依赖包 find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(example_interfaces REQUIRED)2）添加编译可执行程序及对应的源码 add_executable(action_client_member_functions src/member_functions_cli.cpp) ament_target_dependencies(action_client_member_functions "rclcpp" "rclcpp_action" "example_interfaces") add_executable(action_server_member_functions src/member_functions_srv.cpp) ament_target_dependencies(action_server_member_functions "rclcpp" "rclcpp_action" "example_interfaces")3）添加安装规则 install(TARGETS action_client_member_functions action_server_member_functions DESTINATION lib/${PROJECT_NAME})4）完整的 CMakeLists.txt 内容如下 ~/ros/src/laoerExample/cAction$ cat CMakeLists.txt cmake_minimum_required(VERSION 3.8) project(cAction) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(example_interfaces REQUIRED) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() add_executable(action_client_member_functions src/member_functions_cli.cpp) ament_target_dependencies(action_client_member_functions "rclcpp" "rclcpp_action" "example_interfaces") add_executable(action_server_member_functions src/member_functions_srv.cpp) ament_target_dependencies(action_server_member_functions "rclcpp" "rclcpp_action" "example_interfaces") if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() install(TARGETS action_client_member_functions action_server_member_functions DESTINATION lib/${PROJECT_NAME}) ament_package() 3、编译在工作空间根目录中编译： cd ~/ros ~/ros$ colcon build 4、运行在终端1中运行服务端： source install/setup.sh ros2 run cAction action_server_member_functions在终端2中运行客户端： source install/setup.sh ros2 run cAction action_client_member_functions终端1服务端打印信息如下： ~$ ros2 run cAction action_server_member_functions [INFO] [1686017247.134106327] [minimal_action_server]: Received goal request with order 10 [INFO] [1686017247.134329634] [minimal_action_server]: Executing goal [INFO] [1686017247.134416312] [minimal_action_server]: Publish Feedback …… [INFO] [1686017255.134476702] [minimal_action_server]: Publish Feedback [INFO] [1686017256.134540042] [minimal_action_server]: Goal Succeeded终端2客户端打印信息如下： ~$ ros2 run cAction action_client_member_functions [INFO] [1686017247.133888720] [minimal_action_client]: Sending goal [INFO] [1686017247.134327098] [minimal_action_client]: Goal accepted by server, waiting for result [INFO] [1686017247.134478858] [minimal_action_client]: Next number in sequence received: 1 …… [INFO] [1686017255.134584821] [minimal_action_client]: Next number in sequence received: 55 [INFO] [1686017256.134662635] [minimal_action_client]: Result received [INFO] [1686017256.134692702] [minimal_action_client]: 0 …… [INFO] [1686017256.134740934] [minimal_action_client]: 55 5、测试1）查看节点信息 在终端3中查看节点信息，运行命令：ros2 node list $ ros2 node list /minimal_action_client /minimal_action_server2）查看动作信息 在终端3中查看话题信息，运行命令：ros2 action list $ ros2 action list /fibonacci |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |